#include "motor.h"
#include "GQueue.h"
#include "leds.h"
#define DBG_TAG "motor"
#define DBG_LVL DBG_ERROR
#include <rtdbg.h>

// 负:顺时针
#define FFB_HOME_TORQUE         -200

rt_thread_t tid_canopenMotor = RT_NULL;
rt_thread_t tid_motorHome[TOTAL_MOTORS] = {RT_NULL};
rt_sem_t sem_motorReady[TOTAL_MOTORS] = {RT_NULL};
rt_sem_t sem_absMoveRcv = RT_NULL;
abs_move_buf_t	absMoveBuf[16];
abs_move_buf_t absMove;
gqueue_t qAbsMove;
// 以下的nodeId与canopenMotors的内容一一对应

home_parameter_t homeParam[TOTAL_MOTORS] =
{
     {.nodeId = CANOPEN_MOTOR_1_NODEID, .returnMode = TQ_MODE,
      .tq.torque = FFB_HOME_TORQUE, .tq.maxVel = FFB_SPEED_DEFAULT,
      .tq.stopTime = 500},

     {.nodeId = CANOPEN_MOTOR_2_NODEID, .returnMode = PP_MODE,
      .pp.pulseFast = SHAKE1_PULSE_FAST, .pp.velFast = SHAKE1_VEL_FAST,
      .pp.pulseSlow = SHAKE1_PULSE_SLOW, .pp.velSlow = SHAKE1_VEL_SLOW,
      .pp.velStop = SHAKE1_VEL_STOP},

      {.nodeId = CANOPEN_MOTOR_3_NODEID, .returnMode = PP_MODE,
      .pp.pulseFast = SHAKE2_PULSE_FAST, .pp.velFast = SHAKE2_VEL_FAST,
      .pp.pulseSlow = SHAKE2_PULSE_SLOW, .pp.velSlow = SHAKE2_VEL_SLOW,
      .pp.velStop = SHAKE2_VEL_STOP},
};

void PP_Home_Thread_Entry(void* parameter);
void TQ_Home_Thread_Entry(void* parameter);
extern void Canopen_Motor_Thread_Entry(void* parameter);
void (*homeEntry[TOTAL_MOTORS])(void *parameter) = {TQ_Home_Thread_Entry,PP_Home_Thread_Entry,PP_Home_Thread_Entry};
void Motor_AbsMove_Rcv(void)
{
    if(sem_absMoveRcv != RT_NULL){
        rt_sem_release(sem_absMoveRcv);
    }
}

void Motor_Thread_Entry(void* parameter)
{
	int32_t setTarget = 0;
	uint8_t nodeId = 0;
	const uint32_t motor_rpm[PLATEFORM_MOTORS] =
	{
        SHAKE1_VEL_RATED,
        SHAKE2_VEL_RATED
	};
	gqueue_init(&qAbsMove,absMoveBuf,sizeof(absMoveBuf)/sizeof(abs_move_buf_t),sizeof(abs_move_buf_t));

	sem_absMoveRcv = rt_sem_create("m m rcv", 0, RT_IPC_FLAG_FIFO);
	for(int i=0;i<TOTAL_MOTORS;i++){
	    sem_motorReady[i] = rt_sem_create("m rdy",
                                            0,
                                            RT_IPC_FLAG_FIFO);
	}

	rt_thread_delay(2000);
	tid_canopenMotor = rt_thread_create("co motor",
										Canopen_Motor_Thread_Entry,
										RT_NULL,
										CANOPEN_MOTOR_THREAD_STACK,
										CANOPEN_MOTOR_THREAD_PRIO,
										CANOPEN_MOTOR_THREAD_TIMESLICE);
	if(tid_canopenMotor != RT_NULL)
	{
		rt_thread_startup(tid_canopenMotor);
	}
	rt_thread_delay(3000);
	// 回零线程、信号量等初始化
	for(int i=0;i<TOTAL_MOTORS;i++){
	    char rtName[16] = {0};
	    sprintf(rtName,"h sta%d",i);
	    homeParam[i].sem_home_start = rt_sem_create(rtName, 0, RT_IPC_FLAG_FIFO);
        if(homeParam[i].sem_home_start == RT_NULL){
            LOG_E("sem_home_start %d create fail",i);
        }

        memset(rtName,0,sizeof(rtName));
        sprintf(rtName,"h sig%d",i);
        homeParam[i].sem_home_signal = rt_sem_create(rtName, 0, RT_IPC_FLAG_FIFO);
        if(homeParam[i].sem_home_signal == RT_NULL){
            LOG_E("sem_home_signal %d create fail",i);
        }

        memset(rtName,0,sizeof(rtName));
        sprintf(rtName,"h suc%d",i);
        homeParam[i].sem_home_success = rt_sem_create(rtName, 0, RT_IPC_FLAG_FIFO);
        if(homeParam[i].sem_home_success == RT_NULL){
            LOG_E("sem_home_success %d create fail",i);
        }

        memset(rtName,0,sizeof(rtName));
        sprintf(rtName,"m home%d",i);
        tid_motorHome[i] = rt_thread_create(rtName,
                                            homeEntry[i],
                                            &homeParam[i],
                                            MOTOR_HOME_THREAD_STACK,
                                            MOTOR_HOME_THREAD_PRIO,
                                            MOTOR_HOME_THREAD_TIMESLICE);

        if(tid_motorHome[i]  != RT_NULL){
            rt_thread_startup(tid_motorHome[i] );
        }else{
            LOG_E("motor home %d create fail",i);
        }

	}

	for(;;)
	{
		//绝对位置的执行 两个电机
	    rt_sem_take(sem_absMoveRcv, RT_WAITING_FOREVER);
		if(gqueue_peek(&qAbsMove,&absMove) == 0)			//有新的位置数据
		{
            setTarget = absMove.target + absMove.pMotor->homePul;
            nodeId = absMove.pMotor->nodeId;
            Canopen_Motor_Set_TargetPos(nodeId,setTarget,motor_rpm[nodeId-2]);
            gqueue_discard_multiple(&qAbsMove,1);			//取出数据
		}
		rt_thread_delay(5);
	}
}

void Motor_Home_Signal(uint8_t nodeId)
{
    for(int i=0;i<TOTAL_MOTORS;i++){
        if((homeParam[i].nodeId == nodeId) && \
           (homeParam[i].sem_home_signal != RT_NULL)){
            rt_sem_release(homeParam[i].sem_home_signal);
        }
    }

}

void Motor_Home_Rcv(uint8_t nodeId)
{
    for(int i=0;i<TOTAL_MOTORS;i++){
        if((homeParam[i].nodeId == nodeId) && \
           (homeParam[i].sem_home_start != RT_NULL)){
            rt_sem_release(homeParam[i].sem_home_start);
        }
    }
}

void Motor_Home_Success_Signal(uint8_t nodeId)
{
    for(int i=0;i<TOTAL_MOTORS;i++){
        if((homeParam[i].nodeId == nodeId) && \
           (homeParam[i].sem_home_success != RT_NULL)){
            rt_sem_release(homeParam[i].sem_home_success);
        }
    }
}

static void Motor_Home_Fail(uint8_t nodeId,uint32_t stopVel)
{
    Canopen_Motor_Home_Move(nodeId,0,stopVel);
    rt_thread_delay(100);
    Canopen_Motor_HomeStatus_Set(nodeId,BHS_FAILED);
}

static void Motor_Home_Success(uint8_t nodeId,uint32_t stopVel,uint8_t rmode)
{
    rt_thread_delay(300);
    Canopen_Motor_Home_Move(nodeId,0,stopVel);
    rt_thread_delay(800);
    Canopen_Motor_Set_Zero(nodeId,rmode);
    Canopen_Motor_HomeStatus_Set(nodeId,BHS_SUCCESS);
    if(nodeId == 1){
//        Canopen_Config_TQ_OD(1);        // 配置为力矩模式
        Motor_Home_Success_Signal(nodeId);
    }
}

// 此回零方式只适合于位置模式
void PP_Home_Thread_Entry(void* parameter)
{
    uint32_t homeStartTick,homeElapsedTick,homeRemainTick;
    home_parameter_t* param = (home_parameter_t*)parameter;
    while(1)
    {
        rt_sem_take(param->sem_home_start,RT_WAITING_FOREVER);
        rt_sem_control(param->sem_home_signal,RT_IPC_CMD_RESET,0);
        LOG_D("axis %d start home",param->nodeId);

        rt_thread_delay(100);
        homeStartTick = rt_tick_get();
        // 1.快速找原点
        Canopen_Motor_Home_Move(param->nodeId,param->pp.pulseFast,param->pp.velFast);
        LOG_D("axis %d home fast",param->nodeId);
        rt_thread_delay(200);
        rt_sem_control(param->sem_home_signal,RT_IPC_CMD_RESET,0);
        if(rt_sem_take(param->sem_home_signal,MOTOR_HOME_TIMEOUT) == RT_EOK)
        {
            // 2.快速找到原点之后延时300ms停止转动
            rt_thread_delay(300);
            Canopen_Motor_Home_Move(param->nodeId,0,param->pp.velStop);
            // 3.反向慢速找原点
            rt_thread_delay(500);
            rt_sem_control(param->sem_home_signal,RT_IPC_CMD_RESET,0);
            Canopen_Motor_Home_Move(param->nodeId,param->pp.pulseSlow,param->pp.velSlow);
            LOG_D("axis %d home slow",param->nodeId);
        }
        else
        {
            LOG_D("axis %d home fail ,no signal",param->nodeId);
            Motor_Home_Fail(param->nodeId,param->pp.velStop);
            goto homeover;
        }

        // 4.超时停止或者成功停止
        homeElapsedTick = rt_tick_get() - homeStartTick;
        if(homeElapsedTick > MOTOR_HOME_TIMEOUT)
        {
            LOG_D("axis %d home fail timeout",param->nodeId);
            Motor_Home_Fail(param->nodeId,param->pp.velStop);
            goto homeover;
        }
        homeRemainTick = MOTOR_HOME_TIMEOUT - homeElapsedTick;
        rt_sem_control(param->sem_home_signal,RT_IPC_CMD_RESET,0);
        if(rt_sem_take(param->sem_home_signal,homeRemainTick) == RT_EOK)
        {
            LOG_D("axis %d home success",param->nodeId);
            Motor_Home_Success(param->nodeId,param->pp.velStop,param->returnMode);
            rt_sem_release(param->sem_home_success);
        }
        else
        {
            LOG_D("axis %d home fail ,no signal",param->nodeId);
            Motor_Home_Fail(param->nodeId,param->pp.velStop);
        }

        homeover:
        homeRemainTick = 0;

    }
}

/*
 * @brief this method is for tq mode with hard limit switch
 *  drive the motor turn clockwise or counter clockwise with a given torque.
 *  check the motor speed continuously,when speed is keep nearly zero for
 *  given time ,the motor is home success.
 */
void TQ_Home_Thread_Entry(void* parameter)
{
    uint32_t homeStartTick = 0,homeStableStartTick = 0;
    int homeResult = 0,vel = 0;
    home_parameter_t* param = (home_parameter_t*)parameter;
    while(1)
    {
        rt_sem_take(param->sem_home_start,RT_WAITING_FOREVER);
        LOG_D("axis %d start home",param->nodeId);

        homeStartTick = rt_tick_get();
        // 朝一个方向旋转找原点
        Canopen_Motor_Set_TargetTorque(param->nodeId,param->tq.torque,param->tq.maxVel);
        while(1)
        {
            if(rt_tick_get() - homeStartTick >= MOTOR_HOME_TIMEOUT){
                homeResult = -1;        // 超时
                LOG_D("axis %d home timeout",param->nodeId);
                break;
            }
            vel = Canopen_Motor_Get_Vel(param->nodeId);
            vel = vel<0?-vel:vel;
            if(vel<10){     // 小于1rpm认为是到了限位
                if(homeStableStartTick == 0){
                    homeStableStartTick = rt_tick_get();
                }
                if(rt_tick_get() - homeStableStartTick >= param->tq.stopTime){
                    homeResult = 1;
                    break;
                }
            }else{
                homeStableStartTick = 0;
            }
            rt_thread_delay(100);
        }

        if(homeResult == 1){
            LOG_D("axis %d home success",param->nodeId);
            Canopen_Motor_Set_Zero(param->nodeId,param->returnMode);
            rt_sem_release(param->sem_home_success);
        }else{

        }
    }
}

/*
 * 电机1，也就是力反馈电机回零不用感应开关
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
//    if(GPIO_Pin == DI_1_Pin){
//        if(Canopen_Motor_HomeStatus_Get(CANOPEN_MOTOR_1_NODEID) > BHS_START){
//            Motor_Home_Signal(CANOPEN_MOTOR_1_NODEID);
//        }
//    }
    if(GPIO_Pin == DI_1_Pin){
        if(Canopen_Motor_HomeStatus_Get(CANOPEN_MOTOR_2_NODEID) > BHS_START){
            Motor_Home_Signal(CANOPEN_MOTOR_2_NODEID);
        }
    }
    if(GPIO_Pin == DI_2_Pin){
        if(Canopen_Motor_HomeStatus_Get(CANOPEN_MOTOR_3_NODEID) > BHS_START){
            Motor_Home_Signal(CANOPEN_MOTOR_3_NODEID);
        }
    }
}

